#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include <string.h>

/* Scheduler include files. */
#include "FreeRTOS.h"
#include "task.h"
#include "projdefs.h"
#include "semphr.h"

/* include files. */
#include "vtUtilities.h"
#include "vtI2C.h"
#include "drive.h"
#include "LCDtask.h"
//#include "i2cAdc.h"
#include "I2CTaskMsgTypes.h"
#include "motorControl.h"

#define vtDriveQLen 10 

typedef struct __vtDriveMsg {
	uint8_t msgType;
	uint8_t	length;	 // Length of the message to be printed
	uint8_t front;
	uint8_t	left;
	uint8_t	right;
	uint8_t finishline;
	int leftEncoder;
	int rightEncoder;
	//uint8_t buf[vtdriveMaxLen+1]; // On the way in, message to be sent, on the way out, message received (if any)
} vtDriveMsg;

#define baseStack 3
#if PRINTF_VERSION == 1
#define i2cSTACK_SIZE		((baseStack+5)*configMINIMAL_STACK_SIZE)
#else
#define i2cSTACK_SIZE		(baseStack*configMINIMAL_STACK_SIZE)
#endif

static portTASK_FUNCTION_PROTO( vDriveUpdateTask, pvParameters );

static vtDriveStruct* driveStructStatic = 0;
//public API
//void vStartDriveTask(vtDriveStruct* params, unsigned portBASE_TYPE uxPriority, vtLCDStruct *lcd) //, vtMotorStruct *motor);
void vStartDriveTask(vtDriveStruct* params, unsigned portBASE_TYPE uxPriority, vtLCDStruct *lcd) //,vtLCDStruct *lcd, vtMotorStruct *motor);
{
	//create the queue to talk to this task
	if((params->inQ = xQueueCreate(vtDriveQLen,sizeof(vtDriveMsg))) == NULL){
		 VT_HANDLE_FATAL_ERROR(0);
	}
	//start the task
	portBASE_TYPE retval;
	params->lcdData = lcd;
	if ((retval = xTaskCreate( vDriveUpdateTask, ( signed char * ) "drive", i2cSTACK_SIZE, (void *) params, uxPriority, ( xTaskHandle * ) NULL )) != pdPASS) {
		VT_HANDLE_FATAL_ERROR(retval);
	}

	driveStructStatic = params;

}

//portBASE_TYPE SendDriveValueMsg(vtDriveStruct* driveData, portTickType ticksElapsed,portTickType ticksToBlock)
portBASE_TYPE SendDriveValueMsg(vtDriveStruct* driveData, uint8_t msg_type, uint8_t front, uint8_t left, uint8_t right, uint8_t finish, portTickType ticksToBlock)
{

	if (driveData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
					                
	vtDriveMsg driveBuffer;
	/*driveBuffer.length = sizeof(ticksElapsed);
		if (driveBuffer.length > vtdriveMaxLen) {
		// no room for this message
		VT_HANDLE_FATAL_ERROR(driveBuffer.length);
	}  */
//	char val[10];
//	sprintf(val, "%d", 2);
//	driveBuffer.length = strnlen(val,vtdriveMaxLen);
//	strncpy((char *)driveBuffer.buf,val,sizeof(val));
//	driveBuffer.msgType = 0;
	//memcpy(driveBuffer.buf,(char *)&ticksElapsed,sizeof(ticksElapsed));
	driveBuffer.front = front;
	driveBuffer.left = left;
	driveBuffer.right = right;
	driveBuffer.finishline = finish;
	driveBuffer.msgType = msg_type;
	if(xQueueSend(driveData->inQ,(void *) (&driveBuffer), ticksToBlock)){
//	if(true){
		//printf("Q is sending");
		return pdTRUE;
	}
	else{
		//printf("Q isnt sending\n");	
		return pdFALSE;
	}
}

portBASE_TYPE SendEncoderValueMsg(vtDriveStruct* driveData, uint8_t msg_type, int le, int re, portTickType ticksToBlock)
{

	if (driveData == NULL) {
		VT_HANDLE_FATAL_ERROR(0);
	}
					                
	vtDriveMsg driveBuffer;
	/*driveBuffer.length = sizeof(ticksElapsed);
		if (driveBuffer.length > vtdriveMaxLen) {
		// no room for this message
		VT_HANDLE_FATAL_ERROR(driveBuffer.length);
	}  */


	driveBuffer.leftEncoder = le;
	driveBuffer.rightEncoder = re;
	driveBuffer.msgType = msg_type;
	if(xQueueSend(driveData->inQ,(void *) (&driveBuffer), ticksToBlock)){
//	if(true){
		//printf("Q is sending");
		return pdTRUE;
	}
	else{
		//printf("Q isnt sending\n");	
		return pdFALSE;
	}
}




uint8_t convertIRSensor(uint8_t val)
{
	int v = ((int)val);
	int m, b;
	if(v>102)
	{
		m = -49;
		b = 11000;
	}
	else if(v>64)
	{
		m = -105;
		b = 16667;
	}
	else if(v>48)
	{
		m = -261;
		b = 26667;
	}
	else if(v>33)
	{
		m = -392;
		b = 33000;
	}	
	else if(v>22)
	{
		m = -891;
		b = 49545;
	}
	else if(v>15)
	{
		m = -1508;
		b = 63077;
	}
	else
	{
		return 40;
	}
	


	return (uint8_t)((v*m+b)/1000);
}

void sendSensorData(uint8_t front, uint8_t left, uint8_t right, uint8_t finishline)
{

	
	if(driveStructStatic!=0)
	   SendDriveValueMsg(driveStructStatic, 1, front, left, right, finishline, 0);
}

void sendEncoderData(int left, int right)
{

	if(driveStructStatic!=0)
		SendEncoderValueMsg(driveStructStatic, 3, left, right, 0);
}


void sendWebCommand(uint8_t start, uint8_t stop, uint8_t optimized)
{
	if(driveStructStatic!=0)
	   SendDriveValueMsg(driveStructStatic, 2, start, stop, optimized, 0, 0);

}


//End Public API
/*-----------------------------------------------------------*/
int getMsgType(vtDriveMsg *Buffer){
 	return(Buffer->msgType);
}

uint8_t getFront(vtDriveMsg *Buffer)
{
	return(Buffer->front);
}

uint8_t getLeft(vtDriveMsg *Buffer)
{
	return(Buffer->left);
}

uint8_t getRight(vtDriveMsg *Buffer)
{
	return(Buffer->right);
}


//if recenter =1 ->recenter right, 2 ->recenter left
uint8_t reCenter=0;
const uint8_t fsmStateStart1 = 0;
const uint8_t fsmStateRecenter = 1;
const uint8_t fsmStateStraight = 2;
const uint8_t fsmStateRight = 3;
const uint8_t fsmStateLeft = 4;
const uint8_t fsmStateStart2 = 5;
const uint8_t fsmStateStop1 = 6;
const uint8_t fsmStateStop2 = 7;
const uint8_t fsmStateSlow = 8;
const uint8_t fsmStateIdle = 9;
const uint8_t fsmStateRecover = 10;
const uint8_t fsmStateToggle = 11;
const uint8_t fsmStateOptim = 12;
const uint8_t fsmStateBackup = 13;



static portTASK_FUNCTION( vDriveUpdateTask, pvParameters )
{
	//srand(time(NULL));
	// Get the parameters
	vtDriveStruct *param = (vtDriveStruct *) pvParameters;
	// Get the LCD information pointer
	vtLCDStruct *lcdData = param->lcdData;
	// String buffer for printing
	char lcdBuffer[vtLCDMaxLen+1];
	char front_lcdBuffer[vtLCDMaxLen+1];
	char right_lcdBuffer[vtLCDMaxLen+1];
	char left_lcdBuffer[vtLCDMaxLen+1];
	char encode_lcdBuffer[vtLCDMaxLen+1];
	char finish_lcdBuffer[vtLCDMaxLen+1];
	// Buffer for receiving messages
	vtDriveMsg msgBuffer;
	int currentState;
	int count;
	int webControl;
	int rcCount;
	int toggleCount;
	int optRun;
	int optimCounter;
	int rtogg;
	int ltogg;
	const uint8_t arrSize = 20;
	int optimStore[arrSize][2];	  // second dimention denotes left then right encoder values
	int sizeOfStore = -1;
	uint8_t currentStoreIndex = 0;
	rtogg = 0;
	ltogg =	0;
	uint8_t previousFSMOptim;
	uint8_t previousFSMToggle;
	uint8_t reCenter;	   //1 for to the left, 2 for recentering to the right
	uint8_t Front;
	uint8_t Left;
	uint8_t Right;

	uint16_t aFront;
	uint16_t aLeft;
	uint16_t aRight;

	uint8_t lastFront;
	uint8_t lastLeft;
	uint8_t lastRight;

	int rEncoder = 0;
	int	lEncoder = 0;

	previousFSMToggle = 0;

	uint8_t start;
	uint8_t stop;
	uint8_t optimized;
	
	uint8_t init;

	//uint16_t frontArray[16]= {50, 50, 50, 40, 30, 20, 10,  /*left*/ 84, 74, 64, 56  /*ReCenter*/, 44, 30, 20, 9,  4  };
	//uint16_t leftArray[16] = {15, 15, 15, 13, 13, 11, 50,  /*left*/ 10, 8,  6,  3   /*ReCenter*/, 14, 13, 12, 13, 14 };
	//uint16_t rightArray[16] = {15, 15, 15, 17, 17, 19, 19, /*left*/ 17, 19, 21, 24  /*ReCenter*/, 13, 14, 15, 14, 50 };

	//RC 0= right, 1 = left
	optRun = 0;
	optimCounter = 0;
	reCenter = 0;

	previousFSMOptim = 0;

	aFront = 0;
	aLeft = 0;
	aRight = 0;

	uint8_t fwd = 50;
	toggleCount=0;
	uint8_t rwd = 78;
	uint8_t speedmax = 24;
	uint8_t driveConst = 50;
	uint8_t finish = 100;
	//lastFront;
	//lastLeft;
	//lastRight;
	
	currentState = fsmStateStart1;
	count=0;
	init = 0;
	webControl = 1;
	//countLimit;
	//Tasks shouldnt exit	
	for(;;)
	{
		  if (xQueueReceive(param->inQ,(void *) &msgBuffer, portMAX_DELAY) != pdTRUE) { 
		  //	printf("Q isnt pulling out \n");
			VT_HANDLE_FATAL_ERROR(0);
		  }

		  	if(getMsgType(&msgBuffer) == 3)
			{
				rEncoder += msgBuffer.rightEncoder;
				lEncoder += msgBuffer.leftEncoder;
			}
		  	
			if((getMsgType(&msgBuffer) == 1) || (init == 0) ){
				finish = msgBuffer.finishline;
				Front = getFront(&msgBuffer);
				Left = convertIRSensor(getLeft(&msgBuffer));
				Right = convertIRSensor(getRight(&msgBuffer));
				init = 1;
				if( (previousFSMToggle == 1) && (optRun == 0) ){
					if(sizeOfStore < arrSize)
					{
						if(currentState == fsmStateRecenter){
							optimStore[sizeOfStore][0]= ltogg;
							optimStore[sizeOfStore][1]= rtogg;
						}
						else{
							optimStore[sizeOfStore][0]= lEncoder;
							optimStore[sizeOfStore][1]= rEncoder;
						}
					}
				}
				if(finish<=40)
				{
						currentState = fsmStateStop1;
				}
				else if( (currentState == fsmStateStraight) || (currentState == fsmStateToggle) || (currentState == fsmStateBackup) ){
				rcCount = 0;	 		
					if(Front > 125){
						currentState = fsmStateBackup;
					}
					else if(Front > 82){
						currentState = fsmStateSlow;
					}
					else if(Right <= 14){
						reCenter = 0;
						currentState = fsmStateRecenter;
					}
					else if(Left <= 14){
						reCenter = 1;
						currentState = fsmStateRecenter;
					}  
					else if( ((Left >= 40) && (Right >= 40)) && (currentState == fsmStateStraight)){
						 rEncoder=0;
						 lEncoder=0;
						 toggleCount=0;
						 if( (optRun == 0) || (currentStoreIndex >= sizeOfStore) ){
						 	sizeOfStore++;
							currentState = fsmStateToggle;
						}
						else{
							currentState = fsmStateOptim;
						}
					}
				}
				else if(currentState == fsmStateOptim){
					optimCounter++;  
					if(Front > 125){
						currentState = fsmStateBackup;
					}
					else if(Front > 82){
						currentState = fsmStateSlow;
					}
					else if(Right <= 14){
						reCenter = 0;
						currentState = fsmStateRecenter;
					}
					else if(Left <= 14){
						reCenter = 1;
						currentState = fsmStateRecenter;
					}
				}
				else if(currentState == fsmStateSlow){
					if(Front > 128){
						currentState = fsmStateBackup;
					}
					else if(  (Right >= 40) ) {
						currentState = fsmStateRight;
					}
					else if(Left >= 40) {
						currentState = fsmStateLeft;
					}
				}
				else if( currentState == fsmStateRight ){
					if (Front < 85){
					 	currentState = fsmStateStart1;
					}
				}
				else if (currentState == fsmStateLeft){
					if (Front < 85){
					 	currentState = fsmStateStart1;
					}
				}
				else if (currentState == fsmStateRecenter){
					/*if (reCenter == 0 ){
					 	if(Right >= 10){
							currentState = fsmStateStart1;
						}
					}
					else  {
						if (Left >= 10){
						   currentState = fsmStateStart1;
						}
					*/
					
						currentState = 	fsmStateStart1;
					
				}
			}
			if(count==16)
			{
				sprintf(front_lcdBuffer, "F_Sensor: %d", Front );
				sprintf(left_lcdBuffer, "L_Sensor: %d cm", Left );
				sprintf(right_lcdBuffer, "R_Sensor: %d cm", Right );
				//sprintf(encode_lcdBuffer, "EnL: %d EnR: %d", lEncoder/100, rEncoder/100 );
				 sprintf(encode_lcdBuffer, "EnL: %d EnR: %d; %d", lEncoder/100, rEncoder/100, (int)sizeOfStore );
				sprintf(finish_lcdBuffer, "Finish_sensor %d", finish );
				if (lcdData != NULL) {
					if (SendLCDPrintMsg(lcdData,strnlen(front_lcdBuffer,vtLCDMaxLen),0,front_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				 				  
				
					if (SendLCDPrintMsg(lcdData,strnlen(left_lcdBuffer,vtLCDMaxLen),1,left_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				
				
					if (SendLCDPrintMsg(lcdData,strnlen(right_lcdBuffer,vtLCDMaxLen),2,right_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}

					if (SendLCDPrintMsg(lcdData,strnlen(encode_lcdBuffer,vtLCDMaxLen),3,encode_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}

					if (SendLCDPrintMsg(lcdData,strnlen(finish_lcdBuffer,vtLCDMaxLen),4,finish_lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				
					if((currentState == 6) && (webControl == 0) ){
						sprintf(lcdBuffer, "FINISH LINE!");			
					}
					else{
						sprintf(lcdBuffer, "State: %d", currentState );
					}

					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),5,lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				}

			}

			if(getMsgType(&msgBuffer) == 2){
				start = getFront(&msgBuffer);
				stop = getLeft(&msgBuffer);
				optimized = getRight(&msgBuffer);
				if (start == 1){
				webControl =0;
					if(optimized == 0){
						/*uint8_t fwd = 50;
						uint8_t rwd = 78;
						speedmax = 24;
						driveConst = 50;
						//setMotorSpeed(50,50);*/ 
						currentState = fsmStateStart1;
						optRun=0;
					}
					else{
						/*uint8_t fwd = 20;
						uint8_t rwd = 100;
						speedmax = 30;
						driveConst = 40;
						//setMotorSpeed(30,30);	  */
						currentState = fsmStateStart1;
						optRun=1;
					}  
				}
				else{
					webControl =1;
					//if(optimized == 0){
						setMotorSpeed(64, 64);
						currentState = fsmStateStop1;
					//}
					if(optimized ==1){
						optRun=1;
					}
					else{
						optRun=0;
					}
				}
			}
			else if(webControl == 0){
				uint8_t sensRightDist = 160 - Right;
				uint8_t sensLeftDist = 160 - Left;

			float correction = ((float)(Right-Left))/(Right+Left);

			//setMotorSpeed((uint8_t)(driveConst+speedmax*correction),(uint8_t)(driveConst-speedmax*correction)); 
			
				if(count == 16){
					if (lcdData != NULL) {
						sprintf(lcdBuffer, "rSpeed: %d", (int)(driveConst+speedmax*correction));
						if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),7,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}
					} 
					if (lcdData != NULL) {
						sprintf(lcdBuffer, "lSpeed: %d", (int)(driveConst-speedmax*correction));
						if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),8,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
						}
					}
				} 
			switch(currentState){
				case (int)fsmStateStart1:
					speedmax = 30;
					driveConst = 45; 
					currentState = fsmStateStraight;
					previousFSMOptim = 0;
					previousFSMToggle = 0;
					break;
				case (int)fsmStateBackup:
					  setMotorSpeed(75,75);
					break;

				case (int)fsmStateStraight:
					if( ((Right <= 20) || (Left < 20)) && (abs(correction) > 10) ){
						   driveConst = 60; 
						   speedmax = 45;
					}  				  
					else if( ((Right <= 25) || (Left < 25)) && (abs(correction) > 10) ){
						   driveConst = 55; 
						   speedmax = 40;
					}  
					else{
						speedmax =30;
						driveConst = 50;  
					}
					setMotorSpeed((uint8_t)(driveConst+speedmax*correction),(uint8_t)(driveConst-speedmax*correction));
					  
					previousFSMOptim = 0;
					//setMotorSpeed(50,50);
					previousFSMToggle = 0;
					break;

				case (int)fsmStateRight:
					setMotorSpeed(84,45);
					previousFSMOptim = 0; 
					previousFSMToggle = 0;
					break;

				case (int)fsmStateLeft:
					setMotorSpeed(45,84);
					previousFSMOptim = 0; 
					previousFSMToggle = 0;
					break;

				case (int)fsmStateSlow:
					driveConst = 55;
					speedmax = 40;
					setMotorSpeed((uint8_t)(driveConst+speedmax*correction),(uint8_t)(driveConst-speedmax*correction));
					previousFSMOptim = 0;
					previousFSMToggle = 0;
					break;

				case (int)fsmStateRecenter:
					if(reCenter == 0)  {
						setMotorSpeed(63,82);
					}
					else{
						setMotorSpeed(82,63);
					}
					previousFSMOptim = 0;
					previousFSMToggle = 0;
					break;

				case (int)fsmStateStop1:
					//turn right
						//untill encoders say 90* -- searching for turn entire time
						//check turn
					//if not, turn back 180*
					//may have to hang if no turn found
					setMotorSpeed(64,64);
					previousFSMOptim = 0;
					previousFSMToggle = 0;
					break;

				case (int)fsmStateStart2: 
					speedmax = 30;
					driveConst = 40;
					currentState = fsmStateStraight;
					previousFSMOptim = 0;
					previousFSMToggle = 0;
					break;

				case (int)fsmStateStop2:
					//turn right
					//check turn
					//if not, turn left
					//may have to hang if no turn found
					setMotorSpeed(64,64);
					previousFSMOptim = 0;
					previousFSMToggle = 0;
					break;

				case (int)fsmStateIdle:
				previousFSMOptim = 0;
				previousFSMToggle = 0;
					break;

				case (int)fsmStateRecover:
					driveConst = 55;
					speedmax = 23;
					setMotorSpeed((uint8_t)(driveConst+speedmax*correction),(uint8_t)(driveConst-speedmax*correction));
					previousFSMOptim = 0;
					previousFSMToggle = 0;
					break;
	
				case (int)fsmStateToggle:
					/*	if( rEncoder < 400){ 
							setMotorSpeed(40, 59);
							toggleCount = lEncoder;
						} 
						else if((lEncoder-toggleCount) < 400){ 
						 setMotorSpeed(59, 40); 
						}
						else{
							rEncoder = 0;
							lEncoder = 0;
							//setMotorSpeed(65, 20);
						}
					*/	
									
						if(toggleCount < 10){
						   setMotorSpeed(50, 50);
						}
					 	else if(toggleCount < 13){
					   	   setMotorSpeed(35, 65);
					  	}
					   	else if(toggleCount < 19){
					   	   setMotorSpeed(65, 32);
					   	}
						else if(toggleCount < 22){
					   	   setMotorSpeed(32, 65);
					   	}
					   	else{
					   		toggleCount=0;
					   	}
					   	toggleCount++;
						
						previousFSMOptim = 0;
						previousFSMToggle = 1;
					break;
				case (int)fsmStateOptim:
					previousFSMOptim = 1;
					previousFSMToggle = 0;
					
					int lDiff = optimStore[currentStoreIndex][0] - lEncoder;
					int rDiff = optimStore[currentStoreIndex][1] - rEncoder;
					if(lDiff<=50 && lDiff>=-50 && rDiff<=50 && rDiff>=-50)
					{
						sizeOfStore++;
						currentState = fsmStateStraight;
					}
					else
					{
						float lMult = ((float)lDiff)/(lDiff+rDiff);
						setMotorSpeed((int)(64-(35*lMult)), (int)(64-(35*(1-lMult))));
					}
				break;

				default:  
					setMotorSpeed(64, 64);
					break;
			}
			if(getMsgType(&msgBuffer) == 2){
				if (currentState == fsmStateStart1){
					sprintf(lcdBuffer, "Start Reg" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),6,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
					}
				}
				else if (currentState == fsmStateStart2){
					sprintf(lcdBuffer, "Start Optim" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),6,lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				}
				else if (currentState == fsmStateStop1){
					sprintf(lcdBuffer, "Stop Reg" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),6,lcdBuffer,portMAX_DELAY) != pdTRUE) {
							VT_HANDLE_FATAL_ERROR(0);
					}
				}
				else if (currentState == fsmStateStop2){
					sprintf(lcdBuffer, "Stop Optim" );
					if (SendLCDPrintMsg(lcdData,strnlen(lcdBuffer,vtLCDMaxLen),6,lcdBuffer,portMAX_DELAY) != pdTRUE) {
						VT_HANDLE_FATAL_ERROR(0);
					}
				}
			}
			if(count == 16){
				count=0;
			}
			count++;
			lastFront = Front;
			lastLeft = Left;
			lastRight = Right;
			}
		}
	}		


						   

